/*
 * Copyright (c) 2011 Jean-Marc Perronne
 * Ensisa, 12 rue des Freres Lumieres, F-68093 Mulhouse cedex
 * All rights reserved.
 *
 * The copyright of this software is owned by Werner Randelshofer. 
 * You may not use, copy or modify this software, except in  
 * accordance with the license agreement you entered into with  
 * Jean-Marc Perronne. For details see accompanying license terms. 
 */

package fr.uha.ensisa.jmp.drone.navdata;

import java.nio.ByteBuffer;
import java.util.ArrayList;

import fr.uha.ensisa.jmp.drone.app.DroneConstants;
import fr.uha.ensisa.jmp.drone.navdata.options.NavdataDemo;
import fr.uha.ensisa.jmp.drone.navdata.options.VisionDetect;
import fr.uha.ensisa.jmp.std.Format;


public class NavdataParser
{
	private int poscks;
	private Navdata navdata;
	
	public  void preparse(ByteBuffer data)
	{
		this.navdata = new Navdata();
		
		int header = data.getInt();
		int state = data.getInt();
		int sequenceNumber = data.getInt();
		int visionFlag = data.getInt();
		
		this.navdata.update(header, sequenceNumber, state, visionFlag);
	}
	
	public  boolean parseOptions(ByteBuffer data)
	{
		ArrayList<NavdataOption> options = new ArrayList<NavdataOption>();

		while(true)
		{
			final int option = Format.unsignedShort(data.getShort());
			switch (option)
			{
				case DroneConstants.NAVDATA_DEMO_TAG :
					//System.out.println(option + " NAVDATA_DEMO_TAG");
					options.add(this.parseNavdataDemo(option, data));
					break;
			
				case DroneConstants.NAVDATA_CKS_TAG :
					//System.out.println(option + " NAVDATA_CKS_TAG");
						
					if (this.parseCheckSum(data) == this.computeCheckSum(data))
					{
						this.navdata.options(options);
						return true;
					}
					return false;
			
				case DroneConstants.NAVDATA_VISION_DETECT_TAG:
					//System.out.println(option + " NAVDATA_VISION_DETECT_TAG");
					options.add(this.parseVsionDetect(option, data));
					break;
				
				default:
					//System.out.println(option + " OTHER_TAG " + option);
					skipOption(data);
					break;
			}
		}
	}
	
	protected NavdataOption parseVsionDetect(int tag, ByteBuffer data)
	{
		VisionDetect vdd = new VisionDetect();
		vdd.id = tag;
		vdd.size = Format.unsignedShort(data.getShort());
		
		vdd.nb_detected = Format.unsignedInt(data.getInt());
		
		for(int i=0; i<vdd.type.length; i++)
		{
			vdd.type[i] = Format.unsignedInt(data.getInt());
		}
		
		for(int i=0; i<vdd.xc.length; i++)
		{
			vdd.xc[i] = Format.unsignedInt(data.getInt());
		}
	
		for(int i=0; i<vdd.yc.length; i++)
		{
			vdd.yc[i] = Format.unsignedInt(data.getInt());
		}

		for(int i=0; i<vdd.width.length; i++)
		{
			vdd.width[i] = Format.unsignedInt(data.getInt());
		}

		for(int i=0; i<vdd.height.length; i++)
		{
			vdd.height[i] = Format.unsignedInt(data.getInt());
		}

		for(int i=0; i<vdd.dist.length; i++)
		{
			vdd.dist[i] = Format.unsignedInt(data.getInt());
		}

		for(int i=0; i<vdd.dist.length; i++)
		{
			vdd.orientation_angle[i] = data.getFloat();
		}
		return vdd;
	}

	protected  NavdataDemo parseNavdataDemo(int tag, ByteBuffer data)
	{
		NavdataDemo ndd = new NavdataDemo();
		ndd.id = tag;
		ndd.size = Format.unsignedShort(data.getShort());
		ndd.ctrl_state = Format.unsignedInt(data.getInt());
		ndd.vbat_flying_percentage = Format.unsignedInt(data.getInt());
		
		ndd.theta = data.getFloat();
		ndd.phi = data.getFloat();
		ndd.psi = data.getFloat();
		
		ndd.altitude = data.getInt();
		
		ndd.vx = data.getFloat();
		ndd.vy = data.getFloat();
		ndd.vz = data.getFloat();
		
		ndd.num_frames = Format.unsignedInt(data.getInt());
		
		ndd.detection_camera_rot = new Matrix33(data);	
		ndd.detection_camera_trans = new Vector31(data);	
		ndd.detection_tag_index = Format.unsignedInt(data.getInt());
		
		ndd.detection_camera_type = Format.unsignedInt(data.getInt());
		
		ndd.drone_camera_rot =  new Matrix33(data);
		ndd.drone_camera_trans = new Vector31(data);
		return ndd;
	}
	
	protected  void skipOption(ByteBuffer data)
	{
		int size = Format.unsignedShort(data.getShort())-4; //size = 2 bytes id + two bytes size + options bytes
		for (int i = 0; i < size ; i++)
		{
	         data.get();
		}
	}

	protected long parseCheckSum(ByteBuffer data)
	{
		this.poscks = data.position()-2; //2 -> checksum tag pos
		Format.unsignedShort(data.getShort()); //size extracting
		return Format.unsignedInt(data.getInt());
	}
	
	protected long computeCheckSum(ByteBuffer data)
	{
		final int position = data.position();
		final int size = this.poscks;
		data.position(0);
		long cks=0;
		for(int i=0; i<size; i++)
		{
			cks += Format.unsignedByte(data.get());
		}
		data.position(position);
		return cks;
	}

	public synchronized boolean isValidHeader()
	{
		return this.navdata.isValidHeader();
	}
	
	public synchronized boolean isInBootstrapMode()
	{
		return this.navdata.isInBootstrapMode();
	}

	public synchronized boolean isInNavDemoMode()
	{
		return this.navdata.isInNavDemoMode();
	}

	public synchronized boolean isComWatchdogEnabled()
	{
		return this.navdata.isComWatchdogEnabled();
	}
		
	public synchronized boolean isCommandProcessing()
	{
		return this.navdata.isCommandProcessing();
	}

	public synchronized boolean isSequenceNumberGreaterThan(int number)
	{
		return this.navdata.sequenceNumber() > number;
	}
	
	public synchronized int sequenceNumber()
	{
		return this.navdata.sequenceNumber();
	}

	public synchronized Navdata navadata()
	{
		return this.navdata;
	}
	
	public synchronized String toString()
	{
		return this.navdata.toString();
	}

}
